#pragma once

#include "rclcpp/rclcpp.hpp"
#include <cmath>

#include <geometry_msgs/msg/twist.hpp>
#include <yaml-cpp/yaml.h>
#include <fstream>
#include <agv_msgs/msg/odometry_lite.hpp>

#include <memory>
#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/u_int8_multi_array.hpp>
#include <std_msgs/msg/u_int16_multi_array.hpp>
#include <agv_msgs/msg/agv_event_status.hpp>
#include <thread>
#include "toolKit.h"
#include "vehicle_status.h"
#include "time_wheel.h"
#include "output_interface.h"


#define INDICATORS_NAMESPACE "scene_"

#define TW_MAX_LEVEL   3
#define TW_LEVEL_SLOT  5

enum colorCode
{
	GREEN = 0,
	YELLOW,
	RED,
	BLUE,
	WHITE
};

enum blinkCode
{
	CONSTANT_ON = 0,
	SLEEPY_BLINK,
	SLOW_BLINK,
	NORMAL_BLINK,
	FAST_BLINK,
	HIGHFAST_BLINK
};


struct blink_time {
    int on_time_, off_time_;
};

struct pulse_time {
    TIME_TYPE elapsed_ms, last_ms;
    bool level;
    pulse_time():level(true),
                 elapsed_ms(0), 
                 last_ms(0)
    {
    }
    void reset(void)
    {
      level= true;
      elapsed_ms= 0;
      last_ms= GetTickCount();
    }
};

struct Lamp_s
{
  // 三色灯是否开启/关闭
  using MapLighOutType = std::map<std::string, bool>;
  MapLighOutType color_;
  bool is_blink_;
  blink_time fix_time_;

  int blink_code_;
  int color_code_;
  Lamp_s()
  {
    color_ = {
        {"green", false},
        {"yellow", false},
        {"red", false}};

    is_blink_ = false;
  }
  
  bool get_level(std::string color, bool level)
  {
    // 使用 find 方法查找键
    auto it = color_.find(color);
 
    if (it != color_.end()) {
        // 存在键，输出其对应的值
        return level & color_[color];        
    } else {
        // 如果没有找到键，输出相应信息
        DEBUG_ERROR_OUT("[get_level] not find: " << color);  
        return false;
    }

  }

};
struct Speaker
{
  int channel;
  int volume;
  bool insert;
  // int length_ms;
  // int cycle_ms;
  blink_time fix_time_;
  Speaker():channel(0),
            volume(0),
            insert(0)
  {

  }
  Speaker(int ch, int vol)
  {
    channel = ch;
    volume = vol;
    insert = 0;
  }
};

struct Scene
{
  std::string name;
  int index;
  bool trigged;
  Lamp_s lamp_;
  Speaker speaker_;
  
  Scene():trigged(false)
  {
  }
  void trigSet(bool val)
  {
    trigged = val;
  }
  bool trigGet()
  {
    return trigged;
  }
};

class Indicators:public rclcpp::Node
{
public:
  using DEVICE_TYPE = std::pair<std::string, int>;
  // 名字：索引
  using MapOutputType = std::map<std::string, int>;
  using MapIterType = MapOutputType::iterator;
  typedef std::shared_ptr< gallnut::TimeWheel<TW_MAX_LEVEL, TW_LEVEL_SLOT>> TWPtr;
  Indicators():
      Node("Indicators"),
      blink_times_({
        {0, 0},
        {2000, 100},
        {2000, 2000},
        {1000, 1000},
        {500, 500},
        {200, 200}}),
      vehicle_(&scenes_),
      debugLamp_(false),
      debugLampScene_(0),
      debugSpeaker_(false),
      debugSpeakerChannel_(0),
      debugSpeakerVolume_(0),
      outputs_time_({50, 5000})
  { 
    initParam();
    time_wheel_= std::make_shared< gallnut::TimeWheel<TW_MAX_LEVEL, TW_LEVEL_SLOT>>(1);
    spk_pulse_.reset();
    lamp_pulse_.reset();
  }

 ~Indicators();

private:
  bool  debugLamp_,
        debugSpeaker_;
  int   debugLampScene_,
        debugSpeakerChannel_,
        debugSpeakerVolume_;   
  pulse_time  spk_pulse_,
              lamp_pulse_,
              outputs_pulse_;  // 输出5s全部输出一次          
  std::mutex mutex_;
   
  std::thread thread_;
	std::timed_mutex quit_mutex_;

  rclcpp::Subscription<std_msgs::msg::UInt16MultiArray>::SharedPtr debug_sub_;
  rclcpp::Publisher<std_msgs::msg::UInt8MultiArray>::SharedPtr state_pub_;

  // 灯光设置参数
  std::vector<blink_time> blink_times_; 
  blink_time outputs_time_;
  rclcpp::TimerBase::SharedPtr timer_;
  TWPtr time_wheel_;
  OutputInterface outputs_;
public:
  // 所有场景集合
  std::vector<Scene> scenes_; 
  VehicleStatus vehicle_;
public:
  bool pulse_generator(blink_time timeSet, pulse_time &pulse);
  void parserLampCODE(int blink_code, int col, Lamp_s &lamp);
  void debug_Callback(const std_msgs::msg::UInt16MultiArray::ConstSharedPtr msg);
  bool timer_ON(Scene *ptr);
  bool readParam();
  void timerCycleFunc(void);
  void statePub( int index, bool gr, bool ylw, bool red, int ch, int vol);  
  void tw_appendSound( int scene, int delay, std::function<void(int)> func);
  Scene* scene_evaluate(void);
  void thread_cycle(void);
  static void thread_fun(Indicators *pThis);
  void init();
  bool initParam();
};
